#ifndef _FORK_CONTROLLER_H__
#define _FORK_CONTROLLER_H__

#pragma once

#include "rclcpp/rclcpp.hpp"
#include <cmath>
#include <string>
#include <sensor_msgs/msg/joint_state.hpp>
#include <std_msgs/msg/float64.hpp>
#include <agv_srvs/srv/follow_trigger.hpp>
#include <agv_peripheral_controller/fork/peripheral_fork.h>
#include <chassis_controller/pid.h>
#define DEADZ_DECLINE			3
#define LIFT_MAX_SPEED_UP		100
#define LIFT_MAX_SPEED_DOWN  	-200

namespace agv_peripheral
{
	class ForkController
	{
	public:
		ForkController();
		~ForkController(){};

		enum TYPE 
		{
			IDLE,
			TASK,
			MANUAL
		} state_type;


		enum PeriphType
		{
	    	JACKING,
	      	ROTARY,
	      	JACKING_ROTARY,
		    ROLLER,
		    FORKLIFT,
		    ZAPI_CART,
		    YUFENG_CART,
		} peripheral_type;
		
		void init();
		int get_state() { return state; };
		void set_target_position(double pos, double acc = 50);
		void set_target_velocity(double vel);
		void stop();
		double get_current_position() { return current_pos;};
		void update();

	private:
		int  		state;
		bool 		find_joint_id;
		int  		forkjoint_id;
		int  		output = 0;
		double  	max_pos;
		double  	min_pos;
		double 		current_pos;
		double 		current_vel;
		double 		target_pos;
		double 		target_vel;

		rclcpp::Time 					target_set_time;
		rclcpp::Time 					jointState_update_time;
		rclcpp::Node 			nh_;
        ros::Publisher 				forklift_pub;
        ros::Subscriber 			jointstate_sub;
        ros::ServiceServer 			trigger_server;
        std_msgs::msg::Float64 			forklift_data;
		sensor_msgs::msg::JointState 	joint_state;
		PID pid_controller;

		double pos_table[5];
		/*
		double vel_err[10];
		double vel_err_integral;
		double v_kp = 20.0;
        double v_ki = 0.5;
        double v_kd = 2.0;
		*/

		int up_comp;
		int down_comp;

		double pos_err[20];
		double pos_err_integral;
		double p_kp = 20.0;
        double p_ki = 0.5;
        double p_kd = 2.0;
        double forklift_control_acc = 80.0;
        double forklift_control_vel = 80.0;

		double kp = 20.0;
        double ki = 0.5;
        double kd = 2.0;
		double upper_limit = 100.0;
		double lower_limit = -100.0;
		double integral_limit = 30.0;
		double integral_threshold = 0.3;

        bool deadz_detected = false;
        int deadz_up = 0;
        int deadz_down = 0;

		void jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr &msg);
		bool followTriggercallback(agv_srvs::srv::FollowTrigger::Request& req, agv_srvs::srv::FollowTrigger::Response& resp);
		// void forkLiftVelocityControl();
		void forkLiftPositionControl();
	};
}


#endif // _FORK_CONTROLLER_H__
